﻿using MDK.Common;
using System;
using System.Diagnostics;
using System.Threading;

namespace MDK.Device.Components
{
    public interface IAxle : IPlcDeviceComponent
    {
        /// <summary>
        /// 速度,mm/s
        /// </summary>
        double Speed { get; set; }
        /// <summary>
        /// mm
        /// </summary>
        double CurrentCoord { get; }
        double MinimumLimit { get; set; }
        double MaximumLimit { get; set; }
        bool IsZeroCompleted { get; }
        bool HasError { get; }
        int ErrorCode { get; }
        AxleStatus State { get; }
        void StartReturnZero();
        void Goto(double coord);
        bool WaitCompleted(int timeout = int.MaxValue);
        void StartIncrease();
        void StopIncrease();
        void StartDecrease();
        void StopDecrease();
        ISensor MinimumSensor { get; }
        ISensor MaximumSensor { get; }
    }

    public class StdAxle : IAxle
    {
        private PlcAddress _speedAddr;
        private PlcAddress _currentCoordAddr;
        private PlcAddress _zeroCompletedAddr;
        private PlcAddress _returnZeroAddr;
        private PlcAddress _hasErrorAddr;
        private PlcAddress _increaseAddr;
        private PlcAddress _decreaseAddr;
        private PlcAddress _coordAddr;
        private PlcAddress _writeCoordAddr;
        private PlcAddress _triggerAddr;
        private PlcAddress _errorCodeAddr;
        private PlcAddress _zeroSensorAddr;
        private PlcAddress _gotoCompleted;
        private PlcAddress _minSensorAddr;
        private PlcAddress _maxSensorAddr;
        private double _lastGoto;
        private double _lastGotoSrc;

        public StdAxle(IPlcDevice device,
            PlcAddress speedAddr,
            PlcAddress curCoordAddr,
            PlcAddress zeroCompletedAddr,
            PlcAddress returnZeroAddr,
            PlcAddress hasErrorAddr,
            PlcAddress increaseAddr,
            PlcAddress decreaseAddr,
            PlcAddress coordAddr,
            PlcAddress writeCoordAddr,
            PlcAddress triggerAddr,
            PlcAddress errorCodeAddr,
            PlcAddress zeroSensorAddr,
            PlcAddress minSensorAddr,
            PlcAddress maxSensorAddr,
            PlcAddress gotoCompleted
            )
        {
            Device = device;
            UnitMultiple = 10000;
            _speedAddr = speedAddr;
            _currentCoordAddr = curCoordAddr;
            _zeroCompletedAddr = zeroCompletedAddr;
            _returnZeroAddr = returnZeroAddr;
            _hasErrorAddr = hasErrorAddr;
            _increaseAddr = increaseAddr;
            _decreaseAddr = decreaseAddr;
            _coordAddr = coordAddr;
            _writeCoordAddr = writeCoordAddr;
            _triggerAddr = triggerAddr;
            _errorCodeAddr = errorCodeAddr;
            _zeroSensorAddr = zeroSensorAddr;
            _gotoCompleted = gotoCompleted;
            _minSensorAddr = minSensorAddr;
            _maxSensorAddr = maxSensorAddr;
            MinimumSensor = new Sensor(Device, _minSensorAddr);
            MaximumSensor = new Sensor(Device, _maxSensorAddr);
            MinimumLimit = int.MinValue;
            MaximumLimit = int.MaxValue;
        }

        public IPlcDevice Device { get; }

        public int UnitMultiple { get; set; }

        public double Speed
        {
            get { return ((double) Device.PLC.ReadInt(_speedAddr)) / UnitMultiple; }
            set { Device.PLC.WriteInt(_speedAddr, (int)(value * UnitMultiple));}
        }

        public double CurrentCoord
        {
            get { return ((double) Device.PLC.ReadInt(_currentCoordAddr)) / UnitMultiple; }
        }

        public double MinimumLimit { get; set; }
        public double MaximumLimit { get; set; }
        public bool IsZeroCompleted
        {
            get { return Device.PLC.BitIsSet(_zeroCompletedAddr); }
        }

        public bool HasError
        {
            get { return Device.PLC.BitIsSet(_hasErrorAddr); }
        }

        public int ErrorCode
        {
            get { return Device.PLC.ReadInt(_errorCodeAddr); }
        }

        public AxleStatus State { get; private set; }
        public void StartReturnZero()
        {
            Device.PLC.ResetBit(_returnZeroAddr);
            Device.PLC.SetBit(_returnZeroAddr);
            State = AxleStatus.ReturnZero;
        }

        public void Goto(double coord)
        {
            if(coord<MinimumLimit || coord>MaximumLimit)
                throw new ArgumentException();
            _lastGotoSrc = CurrentCoord;
            _lastGoto = coord;
            if (coord == CurrentCoord) return;
            State = AxleStatus.Running;
            if (coord < MinimumLimit)
            {
                coord = MinimumLimit;
            }
            else if (coord > MaximumLimit)
            {
                coord = MaximumLimit;
            }
            Device.PLC.ResetBit(_writeCoordAddr);
            Device.PLC.ResetBit(_triggerAddr);
            Device.PLC.WriteInt(_coordAddr, ((int)(coord * UnitMultiple)));
            Device.PLC.SetBit(_writeCoordAddr);
            Device.PLC.SetBit(_triggerAddr);
            Thread.Sleep(10);
            Device.PLC.ResetBit(_writeCoordAddr);
            Device.PLC.ResetBit(_triggerAddr);
            HandoverData.SendArmData(EnumCmdCode.ArmMove, ((MitsubishiPlc)Device.PLC).PlcIP, this.GetType().Name, _triggerAddr.ToString(), (int)(coord * UnitMultiple), (int)(Speed * UnitMultiple), ErrorCode == 0 ? 0 : 1, ErrorCode);
        }

        public bool WaitCompleted(int timeout = Int32.MaxValue)
        {
            var startTime = DateTime.Now;
            var ts = TimeSpan.FromMilliseconds(timeout);
            var target = Device.PLC.ReadInt(_coordAddr);
            while (DateTime.Now - startTime < ts || timeout == int.MaxValue)
            {
                Thread.Sleep(10);

                if (State == AxleStatus.ReturnZero && Device.PLC.BitIsSet(_zeroCompletedAddr))
                {
                    Device.PLC.ResetBit(_returnZeroAddr);
                    State = AxleStatus.Static;
                    return true;
                }
                
                if (State == AxleStatus.Running && Device.PLC.ReadInt(_currentCoordAddr) == target && Device.PLC.BitIsSet(_gotoCompleted))
                {
                    Device.PLC.ResetBit(_writeCoordAddr);
                    Device.PLC.ResetBit(_triggerAddr);
                    State = AxleStatus.Static;
                    return true;
                }

                if (State == AxleStatus.Static)
                {
                    return true;
                }

                if (State == AxleStatus.Running && (HasError || _lastGotoSrc == Device.PLC.ReadInt(_currentCoordAddr)))
                {
                    Goto(_lastGoto);
                }

                //while (Device.IsScrammed)
                //{
                //    if (FlowEventHelper.OnScrammed(Device))
                //    {
                //        Device.ClearError();
                //        Thread.Sleep(10000);
                //        if (State == AxleStatus.ReturnZero)
                //        {
                //            StartReturnZero();
                //        }
                //        else
                //        {
                //            Goto(_lastGoto);
                //        }
                //        break;
                //    }
                //    else
                //    {
                //        Process.GetCurrentProcess().Kill();
                //    }
                //}
            }

            return false;
        }

        public void StartIncrease()
        {
            Device.PLC.SetBit(_increaseAddr);
            State = AxleStatus.Increasing;
        }

        public void StopIncrease()
        {
            Device.PLC.ResetBit(_increaseAddr);
            State = AxleStatus.Static;
        }

        public void StartDecrease()
        {
            Device.PLC.SetBit(_decreaseAddr);
            State = AxleStatus.Decreasing;
        }

        public void StopDecrease()
        {
            Device.PLC.ResetBit(_decreaseAddr);
            State = AxleStatus.Static;
        }

        public ISensor MinimumSensor { get; }
        public ISensor MaximumSensor { get; }
    }

    public class LocationAxle : IAxle
    {
        private PlcAddress[] _locPCAddrs;
        private PlcAddress _curLocAddr;
        private PlcAddress _zeroAddr;
        private PlcAddress _triggerAddr;
        private PlcAddress _errorAddr;
        private PlcAddress _zeroCompleted;
        private readonly int _realZeroLoc;
        private DateTime _triggerTime;

        public LocationAxle(IPlcDevice device, PlcAddress[] pcs,
            PlcAddress curLocAddr, PlcAddress zeroAddr, PlcAddress triggerAddr, PlcAddress errorAddr, PlcAddress zeroCompleted)
        {
            Device = device;
            _locPCAddrs = pcs;
            _curLocAddr = curLocAddr;
            _zeroAddr = zeroAddr;
            _triggerAddr = triggerAddr;
            _errorAddr = errorAddr;
            _zeroCompleted = zeroCompleted;
            MinimumLimit = int.MinValue;
            MaximumLimit = int.MaxValue;
            _realZeroLoc = (1 << pcs.Length) - 1;
        }

        public IPlcDevice Device { get; }
        public double Speed { get;  set; }

        public double CurrentCoord
        {
            get { return Device.PLC.ReadInt(_curLocAddr); }
        }

        public double MinimumLimit { get; set; }
        public double MaximumLimit { get; set; }
        public bool IsZeroCompleted { get; private set; }

        public bool HasError
        {
            get { return Device != null && !Device.PLC.BitIsSet(_errorAddr); }
        }

        public int ErrorCode { get; private set; }


        public AxleStatus State { get; private set; }


        public void StartReturnZero()
        {
            Device.PLC.ResetBit(_zeroAddr);
            Device.PLC.SetBit(_zeroAddr);
            Thread.Sleep(400);
            Device.PLC.ResetBit(_zeroAddr);
        }

        private double _lastGoto;

        public void Goto(double coord)
        {
            if (coord == 0) coord = _realZeroLoc;

            _lastGoto = coord;
            while (!Device.PLC.BitIsSet(_zeroCompleted))
            {
                Thread.Sleep(100);
            }
            if (CurrentCoord == coord) return;
            if (coord < MinimumLimit)
            {
                coord = MinimumLimit;
            }
            else if (coord > MaximumLimit)
            {
                coord = MaximumLimit;
            }
            var loc = (int)coord;
            Device.PLC.ResetBit(_triggerAddr);
            for (int i = 0; i < _locPCAddrs.Length; i++)
            {
                if ((loc & (1 << i)) == 0)
                {
                    Device.PLC.ResetBit(_locPCAddrs[i]);
                }
                else
                {
                    Device.PLC.SetBit(_locPCAddrs[i]);
                }
            }
            Device.PLC.SetBit(_triggerAddr);
            State = AxleStatus.Running;
            _triggerTime = DateTime.Now;
            Thread.Sleep(400);
            Device.PLC.ResetBit(_triggerAddr);
            HandoverData.SendArmData(EnumCmdCode.ArmMove, ((MitsubishiPlc)Device.PLC).PlcIP, this.GetType().Name, _triggerAddr.ToString(), (int)coord, 7000000, ErrorCode == 0 ? 0 : 1, ErrorCode);
        }

        public bool WaitCompleted(int timeout = Int32.MaxValue)
        {
            var startTime = DateTime.Now;
            var ts = TimeSpan.FromMilliseconds(timeout);
            int loc = (int)_lastGoto;

            while (DateTime.Now - startTime < ts || timeout == int.MaxValue)
            {
                Thread.Sleep(10);
                
                var curLoc = Device.PLC.ReadInt(_curLocAddr);

                if (curLoc == loc)
                {
                    State = AxleStatus.Static;
                    return true;
                }

                if (State == AxleStatus.Running && (DateTime.Now - _triggerTime).TotalMilliseconds > 1000)
                {
                    Goto(_lastGoto);
                }

                //while (Device.IsScrammed || HasError)
                //{
                //    if (FlowEventHelper.OnScrammed(Device))
                //    {
                //        Device.ClearError();
                //        Thread.Sleep(10000);
                //        if (State == AxleStatus.ReturnZero)
                //        {
                //            StartReturnZero();
                //        }
                //        else
                //        {
                //            Goto(_lastGoto);
                //        }
                //        break;
                //    }
                //    else
                //    {
                //        Process.GetCurrentProcess().Kill();
                //    }
                //}
            }
            return false;
        }

        public void StartIncrease()
        {
            throw new NotSupportedException();
        }

        public void StopIncrease()
        {
            throw new NotSupportedException();
        }

        public void StartDecrease()
        {
            throw new NotSupportedException();
        }

        public void StopDecrease()
        {
            throw new NotSupportedException();
        }

        public ISensor MinimumSensor
        {
            get
            {
                throw new NotSupportedException();
            }
        }

        public ISensor MaximumSensor
        {
            get
            {
                throw new NotSupportedException();
            }
        }
    }

    public enum AxleStatus
    {
        Static,
        Increasing,
        Decreasing,
        Running,
        ReturnZero,
        HasError
    }
}